#include "cybertron/glm/function.hpp"
#include "cybertron/gis/UTM.hpp"
#include <cmath>

CYBERTRON_BEGIN

CybertronCoreAPI Deg2UTM::Deg2UTM()
{
	clear();
}

CybertronCoreAPI Deg2UTM::Deg2UTM(double Lat, double Lon)
{
	Zone = (int)std::floor(Lon / 6 + 31);
	build(Lat, Lon, Zone);
}

CybertronCoreAPI Deg2UTM::Deg2UTM(double Lat, double Lon, int Zone)
{
	build(Lat, Lon, Zone);
}

CybertronCoreAPI Deg2UTM::Deg2UTM(const Deg2UTM& rhs)
{
	Easting = rhs.Easting;
	Letter = rhs.Letter;
	Northing = rhs.Northing;
	Zone = rhs.Zone;
	WgsHeight = rhs.WgsHeight;
}

CybertronCoreAPI void Deg2UTM::clear()
{
	Easting = 0;
	Letter = 'A';
	Northing = 0;
	Zone = 0;
	WgsHeight = 0;
}

CybertronCoreAPI Deg2UTM& Deg2UTM::operator = (const Deg2UTM& rhs)
{
	Easting = rhs.Easting;
	Letter = rhs.Letter;
	Northing = rhs.Northing;
	Zone = rhs.Zone;
	WgsHeight = rhs.WgsHeight;

	return *this;
}

CybertronCoreAPI void Deg2UTM::build(double Lat, double Lon, int Zone)
{
	WgsHeight = 0;

	if (Lat < -72)
	{
		Letter = 'C';
	}
	else if (Lat < -64)
	{
		Letter = 'D';
	}
	else if (Lat < -56)
	{
		Letter = 'E';
	}
	else if (Lat < -48)
	{
		Letter = 'F';
	}
	else if (Lat < -40)
	{
		Letter = 'G';
	}
	else if (Lat < -32)
	{
		Letter = 'H';
	}
	else if (Lat < -24)
	{
		Letter = 'J';
	}
	else if (Lat < -16)
	{
		Letter = 'K';
	}
	else if (Lat < -8)
	{
		Letter = 'L';
	}
	else if (Lat < 0)
	{
		Letter = 'M';
	}
	else if (Lat < 8)
	{
		Letter = 'N';
	}
	else if (Lat < 16)
	{
		Letter = 'P';
	}
	else if (Lat < 24)
	{
		Letter = 'Q';
	}
	else if (Lat < 32)
	{
		Letter = 'R';
	}
	else if (Lat < 40)
	{
		Letter = 'S';
	}
	else if (Lat < 48)
	{
		Letter = 'T';
	}
	else if (Lat < 56)
	{
		Letter = 'U';
	}
	else if (Lat < 64)
	{
		Letter = 'V';
	}
	else if (Lat < 72)
	{
		Letter = 'W';
	}
	else
	{
		Letter = 'X';
	}
	Easting = 0.5 *
		std::log(
		(1 + std::cos(Lat * DBL_PI / 180) * std::sin(Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180)) /
			(1 - std::cos(Lat * DBL_PI / 180) * std::sin(Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180))) *
		0.9996 * 6399593.62 /
		std::pow((1 + std::pow(0.0820944379, 2) * std::pow(std::cos(Lat * DBL_PI / 180), 2)), 0.5) *
		(1 +
			std::pow(0.0820944379, 2) / 2 *
			std::pow(
			(0.5 *
				std::log(
				(1 + std::cos(Lat * DBL_PI / 180) * std::sin(Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180)) /
					(1 - std::cos(Lat * DBL_PI / 180) * std::sin(Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180)))),
				2) * std::pow(std::cos(Lat * DBL_PI / 180), 2) / 3) + 500000;
	Easting = std::round(Easting * 100) * 0.01;
	Northing =
		(std::atan(std::tan(Lat * DBL_PI / 180) / std::cos((Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180))) -
			Lat * DBL_PI / 180) * 0.9996 * 6399593.625 /
		std::sqrt(1 + 0.006739496742 * std::pow(std::cos(Lat * DBL_PI / 180), 2)) *
		(1 +
			0.006739496742 / 2 *
			std::pow(
				0.5 *
				std::log(
				(1 + std::cos(Lat * DBL_PI / 180) * std::sin((Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180))) /
					(1 - std::cos(Lat * DBL_PI / 180) * std::sin((Lon * DBL_PI / 180 - (6 * Zone - 183) * DBL_PI / 180)))),
				2) * std::pow(std::cos(Lat * DBL_PI / 180), 2)) +
		0.9996 * 6399593.625 *
		(Lat * DBL_PI / 180 - 0.005054622556 * (Lat * DBL_PI / 180 + std::sin(2 * Lat * DBL_PI / 180) / 2) +
			4.258201531e-05 *
			(3 * (Lat * DBL_PI / 180 + std::sin(2 * Lat * DBL_PI / 180) / 2) +
				std::sin(2 * Lat * DBL_PI / 180) * std::pow(std::cos(Lat * DBL_PI / 180), 2)) / 4 -
			1.674057895e-07 *
			(5 *
			(3 * (Lat * DBL_PI / 180 + std::sin(2 * Lat * DBL_PI / 180) / 2) +
				std::sin(2 * Lat * DBL_PI / 180) * std::pow(std::cos(Lat * DBL_PI / 180), 2)) / 4 +
				std::sin(2 * Lat * DBL_PI / 180) * std::pow(std::cos(Lat * DBL_PI / 180), 2) *
				std::pow(std::cos(Lat * DBL_PI / 180), 2)) / 3);
	if (Letter < 'M')
	{
		Northing = Northing + 10000000;
	}
	Northing = std::round(Northing * 100) * 0.01;
}

CybertronCoreAPI UTM2Deg::UTM2Deg()
{
	clear();
}

CybertronCoreAPI UTM2Deg::UTM2Deg(const UTM2Deg& rhs)
{
	Latitude = rhs.Latitude;
	Longitude = rhs.Longitude;
	Altitude = rhs.Altitude;
}

CybertronCoreAPI UTM2Deg& UTM2Deg::operator = (const UTM2Deg& rhs)
{
	Latitude = rhs.Latitude;
	Longitude = rhs.Longitude;
	Altitude = rhs.Altitude;

	return *this;
}

CybertronCoreAPI void UTM2Deg::clear()
{
	Latitude = 0;
	Longitude = 0;
	Altitude = 0;
}

CybertronCoreAPI void UTM2Deg::build(const Deg2UTM& input)
{
	double Easting = input.Easting;
	double Northing = input.Northing;
	char Letter = input.Letter;
	int Zone = input.Zone;

	Altitude = input.WgsHeight;

	double Hem;
	if (Letter > 'M')
	{
		Hem = 'N';
	}
	else
	{
		Hem = 'S';
	}
	double north;
	if (Hem == 'S')
	{
		north = Northing - 10000000;
	}
	else
	{
		north = Northing;
	}
	Latitude = (north / 6366197.724 / 0.9996 +
		(1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) -
			0.006739496742 * std::sin(north / 6366197.724 / 0.9996) * std::cos(north / 6366197.724 / 0.9996) *
			(std::atan(
				std::cos(
					std::atan(
					(std::exp(
						(Easting - 500000) /
						(0.9996 * 6399593.625 /
							std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
							(1 -
								0.006739496742 *
								std::pow(
								(Easting - 500000) /
									(0.9996 * 6399593.625 /
										std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
									2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) / 3)) -
						std::exp(
							-(Easting - 500000) /
							(0.9996 * 6399593.625 /
								std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
								(1 -
									0.006739496742 *
									std::pow(
									(Easting - 500000) /
										(0.9996 * 6399593.625 /
											std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
										2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) / 3))) / 2 /
						std::cos(
						(north -
							0.9996 * 6399593.625 *
							(north / 6366197.724 / 0.9996 -
								0.006739496742 * 3 / 4 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
								std::pow(0.006739496742 * 3 / 4, 2) * 5 / 3 *
								(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
									std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 -
								std::pow(0.006739496742 * 3 / 4, 3) * 35 / 27 *
								(5 *
								(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
									std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 +
									std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) *
									std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 3)) /
									(0.9996 * 6399593.625 /
										std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
										(1 -
											0.006739496742 *
											std::pow(
											(Easting - 500000) /
												(0.9996 * 6399593.625 /
													std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
												2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) + north / 6366197.724 / 0.9996))) *
				std::tan(
				(north -
					0.9996 * 6399593.625 *
					(north / 6366197.724 / 0.9996 -
						0.006739496742 * 3 / 4 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
						std::pow(0.006739496742 * 3 / 4, 2) * 5 / 3 *
						(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
							std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 -
						std::pow(0.006739496742 * 3 / 4, 3) * 35 / 27 *
						(5 *
						(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
							std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 +
							std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) *
							std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 3)) /
							(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
					(1 -
						0.006739496742 *
						std::pow(
						(Easting - 500000) /
							(0.9996 * 6399593.625 /
								std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
							2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) + north / 6366197.724 / 0.9996)) -
				north / 6366197.724 / 0.9996) * 3 / 2) *
				(std::atan(
					std::cos(
						std::atan(
						(std::exp(
							(Easting - 500000) /
							(0.9996 * 6399593.625 /
								std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
								(1 -
									0.006739496742 *
									std::pow(
									(Easting - 500000) /
										(0.9996 * 6399593.625 /
											std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
										2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) / 3)) -
							std::exp(
								-(Easting - 500000) /
								(0.9996 * 6399593.625 /
									std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
									(1 -
										0.006739496742 *
										std::pow(
										(Easting - 500000) /
											(0.9996 * 6399593.625 /
												std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
											2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) / 3))) / 2 /
							std::cos(
							(north -
								0.9996 * 6399593.625 *
								(north / 6366197.724 / 0.9996 -
									0.006739496742 * 3 / 4 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
									std::pow(0.006739496742 * 3 / 4, 2) * 5 / 3 *
									(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
										std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 -
									std::pow(0.006739496742 * 3 / 4, 3) * 35 / 27 *
									(5 *
									(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
										std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 +
										std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) *
										std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 3)) /
										(0.9996 * 6399593.625 /
											std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
											(1 -
												0.006739496742 *
												std::pow(
												(Easting - 500000) /
													(0.9996 * 6399593.625 /
														std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
													2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) + north / 6366197.724 / 0.9996))) *
					std::tan(
					(north -
						0.9996 * 6399593.625 *
						(north / 6366197.724 / 0.9996 -
							0.006739496742 * 3 / 4 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
							std::pow(0.006739496742 * 3 / 4, 2) * 5 / 3 *
							(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
								std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 -
							std::pow(0.006739496742 * 3 / 4, 3) * 35 / 27 *
							(5 *
							(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
								std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 +
								std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) *
								std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 3)) /
								(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
						(1 -
							0.006739496742 *
							std::pow(
							(Easting - 500000) /
								(0.9996 * 6399593.625 /
									std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
								2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) + north / 6366197.724 / 0.9996)) -
					north / 6366197.724 / 0.9996)) * 180 / DBL_PI;
	Latitude = std::round(Latitude * 10000000);
	Latitude = Latitude / 10000000;
	Longitude =
		std::atan(
		(std::exp(
			(Easting - 500000) /
			(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
			(1 -
				0.006739496742 *
				std::pow(
				(Easting - 500000) /
					(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
					2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) / 3)) -
			std::exp(
				-(Easting - 500000) /
				(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
				(1 -
					0.006739496742 *
					std::pow(
					(Easting - 500000) /
						(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
						2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) / 3))) / 2 /
			std::cos(
			(north -
				0.9996 * 6399593.625 *
				(north / 6366197.724 / 0.9996 -
					0.006739496742 * 3 / 4 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
					std::pow(0.006739496742 * 3 / 4, 2) * 5 / 3 *
					(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
						std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 -
					std::pow(0.006739496742 * 3 / 4, 3) * 35 / 27 *
					(5 *
					(3 * (north / 6366197.724 / 0.9996 + std::sin(2 * north / 6366197.724 / 0.9996) / 2) +
						std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 4 +
						std::sin(2 * north / 6366197.724 / 0.9996) * std::pow(std::cos(north / 6366197.724 / 0.9996), 2) *
						std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) / 3)) /
						(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))) *
				(1 -
					0.006739496742 *
					std::pow(
					(Easting - 500000) /
						(0.9996 * 6399593.625 / std::sqrt((1 + 0.006739496742 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)))),
						2) / 2 * std::pow(std::cos(north / 6366197.724 / 0.9996), 2)) + north / 6366197.724 / 0.9996)) * 180 / DBL_PI +
		Zone * 6 - 183;
	Longitude = std::round(Longitude * 10000000);
	Longitude = Longitude / 10000000;
}

CYBERTRON_END
